#include<modules/MonitorModule/SensorTriggerMonitor.h>
#include "lib/Time.h"
#include "data_struct/base/DataSet.h"

namespace behavior_controller
{
    SensorTriggerMonitor sensor_trigger_monitor;
    void SensorTriggerMonitor::run()
    {
        if(off_ground_switch_trigger())
            DataSet::ctrl_state = SENSOR_TRIGGER_PLAN;
        if(bumper_trigger())
            DataSet::ctrl_state = SENSOR_TRIGGER_PLAN;
 
    }



    bool SensorTriggerMonitor::off_ground_switch_trigger()
    {
        if(DataSet::off_ground_switch.GetTime() > 1e-4)
        {
            if(first_time < 1e-4)
            {
                first_time= DataSet::off_ground_switch.GetTime();
                last_time=first_time;
                count=1;
            }
            else
            {
                if(Time::Now()- first_time >5)
                {
                    first_time=
                    last_time=
                    count=0;
                }
                else if(Time::Now()- first_time >3)
                {                 
                    if(DataSet::off_ground_switch.GetTime()> last_time)
                    {
                        count++;
                        last_time=DataSet::off_ground_switch.GetTime();
                        double frequence= (last_time-first_time)/count;
                        if(frequence> 5)
                        {
                            return true;
                        }
                    }                
                }
                else
                {
                    if(DataSet::bumper_data.GetTime()> last_time)
                    {
                        count++;
                        last_time=DataSet::bumper_data.GetTime();
                    }                   
                }
            }
            return false;
        }
        return false;  
        
    }

    bool SensorTriggerMonitor::bumper_trigger()
    {
        if(DataSet::bumper_data.GetTime() > 1e-4)
        {
            if(first_time_bumper < 1e-4)
            {
                first_time_bumper= DataSet::bumper_data.GetTime();
                last_time_bumper=first_time_bumper;
                count_bumper=1;
            }
            else
            {
                if(Time::Now()- first_time_bumper >5)
                {
                    first_time_bumper=
                    last_time_bumper=
                    count_bumper=0;
                }
                else if(Time::Now()- first_time_bumper >3)
                {                 
                    if(DataSet::bumper_data.GetTime()> last_time_bumper)
                    {
                        count_bumper++;
                        last_time_bumper=DataSet::bumper_data.GetTime();
                        double frequence= (last_time_bumper-first_time_bumper)/count_bumper;
                        if(frequence> 5)
                        {
                            return true;
                        }
                    }                
                }
                else
                {
                    if(DataSet::bumper_data.GetTime()> last_time_bumper)
                    {
                        count_bumper++;
                        last_time_bumper=DataSet::bumper_data.GetTime();
                    }                   
                }
            }
            return false;
        } 
        return false; 
    }
}